#include "../include/Packet.hpp"
#include "../include/util.hpp"

const uint32_t Packet::node[] = {0x4076354B, 0x40A82FC3};

Packet::Packet() {
    packet_length = 0;
    checksum = 0;
}
Packet::~Packet() {}


void Packet::assemble_header(std::vector<uint8_t> * header, std::vector<uint8_t> * payload) {
    header->push_back(get_startByte());
    header->push_back(0xFF00 & payload->size());
    header->push_back(0x00FF & payload->size());
}


/* This should be overridden by all children. */
std::vector<uint8_t> * Packet::get_payload() {
    return NULL;
}


void Packet::createPacket() {
    std::vector<uint8_t> header;

    std::vector<uint8_t> * payload = get_payload();
    assemble_header(&header, payload);

    data.clear();
    data.reserve(header.size() + payload->size() + 1);   /* Plus room for checksum */

    data.insert(data.end(), header.begin(), header.end());
    data.insert(data.end(), payload->begin(), payload->end());
    data.push_back(get_checksum());
}


uint8_t Packet::get_checksum() {
    uint32_t sum = 0;

    /* Skip over the start byte and size bytes. */
    for (uint32_t i = 3; i < data.size(); i++) {
        sum += data[i];
    }

    return (0xFF - (0xFF & sum));
}




bool Packet::is_valid() {
    uint32_t sum = 0;
    std::vector<uint8_t>::iterator it;

    if (data.size() == 0) {
        return (false);
    }

    /* Find location of beginning of packet. */
    for (it = data.begin() ; it != data.end(); ++it) {
        if (*it == startByte) {
            break;
        }
    }

    /* Size bytes are [1] and [2]. */
    packet_length = (0x10 * it[1]) + (it[2]);
    if (data.size() < packet_length + 4) {
        /* Exit early; more data to receive. */
        return (false);
    }

    /* Sum up everything after the start byte & size bytes, excluding the checksum. */
    for (it += 3; it != data.end()-1; ++it) {
        sum += *it;
    }
    std::cout << "last byte: 0x" << hex(it[data.size()]) << std::endl;
    return (0xFF - (0xFF & sum));
}


void push_back_32 (std::vector<uint8_t> * payload, uint32_t data) {
    for (int i = 24; i >= 0; i -= 8) {
        payload->push_back((data >> i) & 0xFF);
    }
}

void push_back_16 (std::vector<uint8_t> * payload, uint16_t data) {
    for (int i = 8; i >= 0; i -= 8) {
        payload->push_back((data >> i) & 0xFF);
    }
}
